#ifndef CAN_TEST_H
#define CAN_TEST_H

  /* ============== *\
  ||                ||
  ||    EXAMPLES    ||
  ||                ||
  \* ============== */

/*
// Include Library
#include <SPI.h>
#include "CAN_lib.h"
*/

/*
// Initialize Example
initCAN();
*/

/*
// Send Example
tCAN msg;
msg.id = 0;
msg.length = 1;
msg.buff[0] = count;
if (sendMessage(&msg)) {
  // message sent
} else {
  // error sending message
}
*/

/*
// Receive Example
tCAN rec;
if (recMessage(&rec)) {
  Serial.print("\tReceived: ");
  Serial.println(rec.buff[0], DEC);
}
*/

#define WRITE_COM     0x02
#define READ_COM      0x03
#define RESET_COM     0x0C
#define STATUS_COM    0xA0
#define STATUS_RX_COM 0xB0

#define SPI_READ_RX   0x90
#define	SPI_WRITE_TX  0x40
#define	SPI_RTS	      0x80

#define CANINTE       0x2B
#define CANINTF       0x2C

#define LOOPBACK_MODE 0x40
#define NORMAL_MODE   0x00
#define CONFIG_MODE   0x80

typedef struct {
  int id;
  int length;
  int rtr;
  char buff[22];
} 
tCAN;

// set pin 10 as the slave select for the digital pot:
const int slaveSelectPin = 49;
//int startupMode = LOOPBACK_MODE;
int startupMode = NORMAL_MODE;


// Public
void initCAN();
int hasMessage(void);
int recMessage(tCAN *msg);
int sendMessage(tCAN *msg);

// Private
void spiReset();
void spiWrite(int address, int value);
char spiWriteContInit(int address);
char spiWriteCont(int data);
int spiRead(int address, int verbose=0);
char spiReadContInit(int address);
char spiReadCont();
char getStatus();
char getRxStatus();


void initCAN() {
  Serial.begin(9600);
  // set the slaveSelectPin as an output:
  pinMode (slaveSelectPin, OUTPUT);
  pinMode (13, OUTPUT);
  // initialize SPI:
  SPI.begin();
  SPI.setDataMode(SPI_MODE0);

  // Configure MCP2515
  spiReset();

  // Set bitrate and speed
  spiWrite(0x28, 0b00000010); // CNF3 <= PHSEG21
  spiWrite(0x29, 0b10010000); // CNF2 <= PHSEG11 | BTLMODE
  spiWrite(0x2A, 0b00000000); // CNF1 <= Speed Same as Occilator

  // Enable interrupts
  spiWrite(CANINTE, 0b00000011); // CANINTE <= Enable inters for 1 and 2
  
  if (spiRead(CANINTE) != 3) {
    Serial.print("Error Setting Interrupts: ");
    Serial.println(spiRead(CANINTE), BIN);
    digitalWrite(13, HIGH);
    while(1);
  }
  
  // Deactivate ouputs
  spiWrite(0x0C, 0b00000000); // deactivate the RXnBF Pins

  // Set inputs
  spiWrite(0x0D, 0b00000000); // set TXnRTS as inputs

  // turn off filters => receive any message
  spiWrite(0x60, 0b01100000); // RXB0CTRL <= RXM1 | RXM0
  spiWrite(0x70, 0b01100000); // RXB0CTRL <= RXM1 | RXM0

  Serial.print("Status: ");
  spiRead(0x0F,1);
  Serial.println("");
  
  // ========================================
  // !!!!!!!   CHANGE BACK TO NORMAL !!!!!!!!
  spiWrite(0x0F, startupMode); // CANCNTRL <= Normal Mode
  // !!!!!!!   CHANGE BACK TO NORMAL !!!!!!!!
  // ========================================

  if (spiRead(0x0F) != startupMode) {
    Serial.print("Error configuring MCP2515\nCANCTRL: ");
    Serial.println(spiRead(0x0F));
    while(1);
  } else if (startupMode == NORMAL_MODE)
    Serial.println("MCP2515 Configured In NORMAL_MODE...");
  else if (startupMode == CONFIG_MODE)
    Serial.println("MCP2515 Configured In CONFIG_MODE...");
  else if (startupMode == LOOPBACK_MODE)
    Serial.println("MCP2515 Configured In LOOPBACK_MODE...");
  else
    Serial.println("MCP2515 Configured Correctly...");
}

/*
 * Returns 1 if RX1 is full
 * Returns 2 if RX2 is full
 * Returns 3 if both RX1 and RX2 are full
 * Zero if no message
 */
int hasMessage(void) {
  return getStatus() & 3; // CANINTF
}

/*
 * Returns the RX buffere which was read
 * Returns 0 if the was an error.
 */
int recMessage(tCAN *msg) {
  int stat = getRxStatus(); // SPI_RX_STATUS
  int addr = 0;
  if (stat & (1 << 6))
    addr = SPI_READ_RX;
  else if (stat & (1 << 7))
    addr = SPI_READ_RX | 4;
  else
    return 0;
  digitalWrite(slaveSelectPin,LOW);
  spiReadContInit(0x90);
  msg->id = spiReadCont() << 3;
  msg->id |= spiReadCont() >> 5;

  spiReadCont();
  spiReadCont();

  msg->length = spiReadCont(); 
  //  message->header.rtr = (bit_is_set(status, 3)) ? 1 : 0; // FROM mcp2515.c
  int i;
  for (i=0;i<msg->length;i++)
    msg->buff[i] = spiReadCont();
  digitalWrite(slaveSelectPin,HIGH);
  delay(2);
  // Clear Interrupt Flag
  spiWrite(CANINTF, stat&(0b11111100|(stat&(1<<6)?0xFE:0xFD)));
  return 1;
}

int sendMessage(tCAN *msg) {
  int stat = getStatus(); // SPI_STATUS
  int addr = 0;
  if (!(stat & (1 << 2)))
    addr = 0x00;
  else if (!(stat & (1 << 4)))
    addr = 0x02;
  else if (!(stat & (1 << 6)))
    addr = 0x04;
  else
    // all buffer used => could not send message
    return 0;
  digitalWrite(slaveSelectPin,LOW);
  spiWriteContInit(addr);
  spiWriteCont(msg->id << 3);
  spiWriteCont(msg->id >> 5);

  spiWriteCont(0);
  spiWriteCont(0);

  // a rtr-frame has a length, but contains no data

  spiWriteCont(msg->length); 
  //  message->header.rtr = (bit_is_set(status, 3)) ? 1 : 0; // FROM mcp2515.c
  int i;
  for (i=0;i<msg->length;i++)
    spiWriteCont(msg->buff[i]);

  digitalWrite(slaveSelectPin,HIGH);
  delay(1);
  addr = (addr == 0) ? 1 : addr;
  digitalWrite(slaveSelectPin,LOW);
  SPI.transfer(SPI_RTS | addr);
  digitalWrite(slaveSelectPin,HIGH);
  return addr;
}

// Private
void spiReset() {
  //Serial.println("\t\t[[ RESETTTING ]]");
  // take the SS pin low to select the chip:
  digitalWrite(slaveSelectPin,LOW);
  //  send in the address and value via SPI:
  SPI.transfer(RESET_COM);
  // take the SS pin high to de-select the chip:
  digitalWrite(slaveSelectPin,HIGH); 
}

// Private
void spiWrite(int address, int value) {
  // take the SS pin low to select the chip:
  digitalWrite(slaveSelectPin,LOW);
  //  send in the address and value via SPI:
  SPI.transfer(WRITE_COM);
  SPI.transfer(address);
  SPI.transfer(value);
  // take the SS pin high to de-select the chip:
  digitalWrite(slaveSelectPin,HIGH); 
}

// Private
char spiWriteContInit(int address) {
  SPI.transfer(WRITE_COM);
  SPI.transfer(address);
}

// Private
char spiWriteCont(int data) {
  return SPI.transfer(data);
}

// Private
int spiRead(int address, int verbose) {
  // take the SS pin low to select the chip:
  digitalWrite(slaveSelectPin,LOW);
  //  send in the address and value via SPI:
  SPI.transfer(READ_COM);
  SPI.transfer(address);
  int data = SPI.transfer(0x00);
  // take the SS pin high to de-select the chip:
  digitalWrite(slaveSelectPin,HIGH);  
  if (verbose) {
    Serial.print(data, BIN);
    Serial.print(", ");
  }
  return data;
}

// Private
char spiReadContInit(int address) {
  SPI.transfer(READ_COM);
  SPI.transfer(SPI_WRITE_TX | address);
}

// Private
char spiReadCont() {
  return SPI.transfer(0x00);
}

// Private
char getStatus() {
  digitalWrite(slaveSelectPin,LOW);
  SPI.transfer(STATUS_COM);
  int data = SPI.transfer(0x00);
  digitalWrite(slaveSelectPin,HIGH);  
  return data;
}

// Private
char getRxStatus() {
  digitalWrite(slaveSelectPin,LOW);
  SPI.transfer(STATUS_RX_COM);
  int data = SPI.transfer(0x00);
  digitalWrite(slaveSelectPin,HIGH);  
  return data;
}

#endif
